#include "pxt.h"
#include "pins.h"

#include "driver/i2c.h"
#include "driver/ledc.h"
#include "esp_task_wdt.h"
#include "esp_log.h"

extern "C" {

static const char *TAG = "PINS";

#include "esp_adc/adc_oneshot.h"
#include "esp_adc/adc_cali.h"
#include "esp_adc/adc_cali_scheme.h"

#define ADC_ATTEN ADC_ATTEN_DB_11
#define ADC_BITS SOC_ADC_RTC_MAX_BITWIDTH

typedef enum {
    PINMODE_NULL = 0,
    PINMODE_INPUT = 1,
    PINMODE_OUTPUT = 2,
    PINMODE_ANALOG = 4,
    PINMODE_TOUCH = 5,
    PINMODE_SERVO = 6,
    PINMODE_PWM = 7,
    PINMODE_I2C = 8,
    PINMODE_SPI = 9,
    PINMODE_EVENT_ON_EDGE = 10,
    PINMODE_EVENT_ON_PULSE = 11,
    PINMODE_EVENT_ON_TOUCH = 12,
    PINMODE_EVENT_ON_COUNT = 13,
    PINMODE_EVENT_ON_COMPARE = 14,
    PINMODE_EVENT_WHEN_CHANGED = 15,
    PINMODE_EVENT_WHEN_HIGH = 16,
    PINMODE_EVENT_WHEN_LOW = 17,
    PINMODE_MAX_MODES = 18,
} pinmode_t;

typedef struct {
    int pin;
    ledc_timer_t timer;
    ledc_channel_t channel;
} ledc_channel_info_t;

static ledc_channel_info_t ledc_channel_info[LEDC_CHANNEL_MAX] = {
    {0xff, LEDC_TIMER_0, LEDC_CHANNEL_0},
    {0xff, LEDC_TIMER_0, LEDC_CHANNEL_1},
    {0xff, LEDC_TIMER_0, LEDC_CHANNEL_2},
    {0xff, LEDC_TIMER_0, LEDC_CHANNEL_3},
    {0xff, LEDC_TIMER_0, LEDC_CHANNEL_4},
    {0xff, LEDC_TIMER_0, LEDC_CHANNEL_5},
    {0xff, LEDC_TIMER_0, LEDC_CHANNEL_6},
    {0xff, LEDC_TIMER_0, LEDC_CHANNEL_7},
};
static bool ledc_timer_initialized[3] = {false, false, false};

// HAL functions port from esp32 ardunio
static bool adc1_initialized = false;
static bool ledc_timer0_initialized = false;
static adc_oneshot_unit_handle_t adc1_handle;
static pinmode_t pin_modes[40];
static bool i2c_initialized = false;

void i2c_master_init() {
    if (i2c_initialized) return;

    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = 2,
        .scl_io_num = 1,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master = {
            .clk_speed = 100000,
        },
    };
    i2c_param_config(I2C_NUM_0, &conf);
    i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0);

    i2c_initialized = true;
}

static void __gpioConfig(int pin, bool isinput){
    gpio_config_t io_conf {
        .pin_bit_mask = 1ULL << pin,
        .mode = isinput ? GPIO_MODE_INPUT : GPIO_MODE_OUTPUT,
    };
    gpio_config(&io_conf);
}

static void __analogInit(int pin, adc_channel_t channel, adc_unit_t unit) {
    adc_unit_t adc_unit;
    if (!adc1_initialized) {
        adc_oneshot_unit_init_cfg_t init_config1 = {
            .unit_id = ADC_UNIT_1,
            .ulp_mode = ADC_ULP_MODE_DISABLE,
        };
        adc_oneshot_new_unit(&init_config1, &adc1_handle);

        ESP_LOGI(TAG, "adc1 initialized %d %d %d", pin, channel, unit);
        adc1_initialized = true;
    }

    adc_oneshot_chan_cfg_t config = {
        .atten = ADC_ATTEN_DB_12,
        .bitwidth = ADC_BITWIDTH_12,
    };

    adc_oneshot_config_channel(adc1_handle, channel, &config);
    // ESP_LOGI(TAG, "adc1 config channel %d %d %d", pin, channel, unit);
    
    // curve fitting only for s3
    adc_cali_handle_t handle = NULL;
    adc_cali_curve_fitting_config_t cali_config = {
        .unit_id = ADC_UNIT_1,
        .chan = channel,
        .atten = ADC_ATTEN_DB_12,
        .bitwidth = ADC_BITWIDTH_12,
    };
    int ret = adc_cali_create_scheme_curve_fitting(&cali_config, &handle);
    // ESP_LOGI(TAG, "adc_cali_create_scheme_curve_fitting %d", ret);
}

static void __ledcInit(int pin, int channel, ledc_timer_t timer, ledc_timer_bit_t duty_resolution, uint32_t freq_hz) {    esp_err_t err = ESP_OK;
    if (!ledc_timer_initialized[timer]) {
        ledc_timer_config_t ledc_timer = {
            .speed_mode = LEDC_LOW_SPEED_MODE,
            .duty_resolution = duty_resolution,
            .timer_num = timer,
            .freq_hz = freq_hz,
            .clk_cfg = LEDC_AUTO_CLK,
        };
        err = ledc_timer_config(&ledc_timer);
        ESP_LOGI(TAG, "ledc_timer_config %d %d %d", pin, channel, err);
        ledc_timer_initialized[timer] = true;
    }

    ledc_channel_config_t ledc_channel = {
        .gpio_num   = pin,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .channel    = (ledc_channel_t)channel, // 0~8
        .timer_sel = timer,
        .duty       = 0,
        .hpoint     = 0,
    };
    err = ledc_channel_config(&ledc_channel);
    ESP_LOGI(TAG, "ledc_channel_config %d %d %d", pin, channel, err);

}

}

namespace pxt {
static DevicePin **pinPtrs;
static uint8_t numPinPtrs;
static uint8_t pinPos[DEV_NUM_PINS];

//% expose
DevicePin *getPin(int id) {

    id &= CFG_PIN_NAME_MSK;

    if (id >= DEV_NUM_PINS)
        soft_panic(PANIC_NO_SUCH_PIN);

    int ptr = pinPos[id];
    if (ptr == 0) {
        pinPtrs = (DevicePin **)realloc(pinPtrs, (numPinPtrs + 1) * sizeof(void *));
        // GCTODO
        pinPtrs[numPinPtrs++] = new DevicePin(id);
        ptr = numPinPtrs;
        pinPos[id] = ptr;
    }
    return pinPtrs[ptr - 1];
}

//% expose
DevicePin *getPinCfg(int key) {
    int p = getConfig(key, -1);
    if (p == -1)
        DMESG("no pin cfg: %d", key);
    return getPin(p);
}

void linkPin(int from, int to) {
    if (from < 0 || from >= DEV_NUM_PINS)
        soft_panic(PANIC_NO_SUCH_PIN);
    getPin(to);
    pinPos[from] = pinPos[to];
}

//% expose
DevicePin *lookupPin(int pinName) {
    if (pinName < 0 || pinName == 0xff)
        return NULL;
    pinName &= CFG_PIN_NAME_MSK;
    return getPin(pinName);
}

//% expose
DevicePin *lookupPinCfg(int key) {
    return lookupPin(getConfig(key));
}

} // namespace pxt

namespace pins {

// enum class PinPullMode {
//     //% block="down"
//     PullDown = 0,
//     //% block="up"
//     PullUp = 1,
//     //% block="none"
//     PullNone = 2
// };

/**
 * Get a pin by configuration id (DAL.CFG_PIN...)
 */
//%
DigitalInOutPin pinByCfg(int key) {
    return pxt::lookupPinCfg(key);
}

/**
 * Create a new zero-initialized buffer.
 * @param size number of bytes in the buffer
 */
//%
Buffer createBuffer(int size) {
    return mkBuffer(NULL, size);
}

/** Microbit like io operations */
//%
Buffer i2cReadBuffer(int address, int size, bool repeat = false) {
    i2c_master_init();
    Buffer buf = mkBuffer(NULL, size);
    registerGCObj(buf);
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_READ, true);
    if (size > 1) {
        i2c_master_read(cmd, buf->data, size - 1, I2C_MASTER_ACK);
    }
    i2c_master_read_byte(cmd, buf->data + size - 1, I2C_MASTER_NACK);
    if (!repeat)
        i2c_master_stop(cmd);
    
    esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmd, 50);
    i2c_cmd_link_delete(cmd);
    unregisterGCObj(buf);

    if (err != ESP_OK) {
        buf = 0;
    }

    return buf;
}

//%
int i2cWriteBuffer(int address, Buffer buf, bool repeat = false) {
    i2c_master_init();
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write(cmd, buf->data, buf->length, true);
    if (!repeat)
        i2c_master_stop(cmd);
    
    esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmd, 50);
    i2c_cmd_link_delete(cmd);

    return err == ESP_OK;
}

//%
int digitalReadPin(int name) {
    if (pin_modes[name] != PINMODE_INPUT) {
        __gpioConfig(name, true);
        pin_modes[name] = PINMODE_INPUT;
    }
    return gpio_get_level((gpio_num_t)name);
}

//%
void digitalWritePin(int name, int value) {
    if (pin_modes[name] != PINMODE_OUTPUT) {
        __gpioConfig(name, false);
        pin_modes[name] = PINMODE_OUTPUT;
    }
    gpio_set_level((gpio_num_t)name, value);
}

//%
int analogReadPin(int name) {
    esp_err_t err;
    adc_channel_t channel;
    adc_unit_t adc_unit;
    err = adc_oneshot_io_to_channel(name, &adc_unit, &channel);
    if (pin_modes[name] != PINMODE_ANALOG) {
        __analogInit(name, channel, adc_unit);
        pin_modes[name] = PINMODE_ANALOG;
    }

    int adc_raw;
    adc_oneshot_read(adc1_handle, channel, &adc_raw);
    ESP_LOGI(TAG, "analogReadPin: %d %ld", name, adc_raw);
    return adc_raw;
}

//%
void analogWritePin(int name, int value) {
    int channel = -1;
    ledc_timer_t timer = LEDC_TIMER_0;

    if (pin_modes[name] != PINMODE_PWM) {
        for (int i = 0; i < LEDC_CHANNEL_MAX; i++) {
            if (ledc_channel_info[i].pin == 0xff) {
                channel = i;
                ledc_channel_info[i].pin = name;
                ledc_channel_info[i].timer = timer;
                ledc_channel_info[i].channel = (ledc_channel_t)channel;
                ESP_LOGI(TAG, "set slot %d %d", i, name);
                break;
            }
        }
        __ledcInit(name, channel, timer, LEDC_TIMER_13_BIT, 50);
        pin_modes[name] = PINMODE_PWM;
    } else {
        for (int i = 0; i < LEDC_CHANNEL_MAX; i++) {
            ESP_LOGI(TAG, "ledc_channel_info[%d].pin = %d", i, ledc_channel_info[i].pin);
            if (ledc_channel_info[i].pin == name) {
                channel = i;
                timer = ledc_channel_info[i].timer;
                break;
            }
        }
    }
    ESP_LOGI(TAG, "analogWritePin: %d %d %d", name, channel, value);
    if (channel == -1) return;
    ledc_set_duty(LEDC_LOW_SPEED_MODE, (ledc_channel_t)channel, value);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, (ledc_channel_t)channel);
}


//%
void servoSetPulse(int name, int micros) {
    int channel = -1;
    ledc_timer_t timer = LEDC_TIMER_1;

    if (pin_modes[name] != PINMODE_SERVO) {
        for (int i = 0; i < LEDC_CHANNEL_MAX; i++) {
            if (ledc_channel_info[i].pin == 0xff) {
                channel = i;
                ledc_channel_info[i].pin = name;
                ledc_channel_info[i].timer = timer;
                ledc_channel_info[i].channel = (ledc_channel_t)channel;
                // ESP_LOGI(TAG, "set slot %d %d", i, name);
                break;
            }
        }
        __ledcInit(name, channel, timer, LEDC_TIMER_13_BIT, 50); // Servo typically uses 50Hz
        pin_modes[name] = PINMODE_SERVO;
    } else {
        for (int i = 0; i < LEDC_CHANNEL_MAX; i++) {
            // ESP_LOGI(TAG, "ledc_channel_info[%d].pin = %d", i, ledc_channel_info[i].pin);
            if (ledc_channel_info[i].pin == name) {
                channel = i;
                timer = ledc_channel_info[i].timer;
                break;
            }
        }
    }
    // ESP_LOGI(TAG, "servoSetPulse: %d %d %d", name, channel, micros);
    if (channel == -1) return;
    int value = micros * 8192 / 20000;
    // ESP_LOGI(TAG, "servo #2: %d %d %d", name, channel, value);
    ledc_set_duty(LEDC_LOW_SPEED_MODE, (ledc_channel_t)channel, value);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, (ledc_channel_t)channel);
}


//%
void setPull(int name, PinPullMode pull) {
    gpio_pull_mode_t _pull = GPIO_FLOATING;
    
    switch (pull) {
        case PinPullMode::PullUp:
            _pull = GPIO_PULLUP_ONLY;
            break;
        case PinPullMode::PullDown:
            _pull = GPIO_PULLDOWN_ONLY;
            break;
        default:
            break;
    }
    
    gpio_set_pull_mode((gpio_num_t)name, _pull);
}

//%
int pulseIn(int name, int pulse, int maxDuration = 200000) {
    uint64_t tick = esp_timer_get_time();
    uint64_t maxd = (uint64_t)maxDuration;
    // set pin to input mode
    if (pin_modes[name] != PINMODE_INPUT) {
        __gpioConfig(name, true);
        pin_modes[name] = PINMODE_INPUT;
    }
    while (gpio_get_level((gpio_num_t)name) != pulse) {
        if (esp_timer_get_time() - tick > maxd)
            return 0;
    }

    uint64_t start = esp_timer_get_time();
    while (gpio_get_level((gpio_num_t)name) == pulse) {
        if (esp_timer_get_time() - tick > maxd)
            return 0;
    }
    uint64_t end = esp_timer_get_time();
    return end - start;
}

} // namespace pins

namespace control {
/**
 * Enable a watchdog timer that need to be fed or it will reset the device.
 * If timeout is not positive, the watchdog is disabled.
 */
//%
void setWatchdog(int timeout_s) {
    printf("setWatchdog: %d\n", timeout_s);
    if (timeout_s > 0) {
        // TODO: rtos already initialize the watchdog
        uint32_t timeout_ms = timeout_s * 1000;
        esp_task_wdt_config_t config = {
            .timeout_ms = timeout_ms,
            .idle_core_mask = 1,
            .trigger_panic = false,
        };
        esp_task_wdt_init(&config);
    } else {
        esp_task_wdt_delete(NULL);
    }
}

/**
 * Reset timeout on previously enabled watchdog.
 */
//%
void feedWatchdog() {
    esp_task_wdt_reset();
}

} // namespace control